package CamposDePotencial;

import java.io.FileInputStream;
import java.io.FileNotFoundException;
import java.io.IOException;
import java.io.InputStream;
import java.util.Properties;

import javax.vecmath.Point3d;

import estrategia.ParametrosPotenciales;
import estrategia.Predictor;
import estrategia.Utiles;
import estrategia.Vector;

public class CampoTangencialObjetivo extends CampoPotencial{

	private double radioCampo;
	private double minAlcance;
	private double Krepulsion;
	private double tamanioRobot;
	
	// parametros de rectas
	double m1, m2, m3, direccionP1, direccionP2, direccionP3;
	
	
	public CampoTangencialObjetivo(int idObjeto, Utiles u, Predictor predictor) {
		// TODO Auto-generated constructor stub
		super(idObjeto, u, predictor);
	}
	
	public void  cargarConstantes() {
	
		radioCampo = ParametrosPotenciales.radioCampoTangencial;
		minAlcance = ParametrosPotenciales.minAlcanceTangencial;
		Krepulsion = ParametrosPotenciales.KCampoTangencial;
		tamanioRobot = ParametrosPotenciales.tamanioRobot;
		
	}
	
	
	/*public double hallarPendienteRecta(Point3d mipos, Point3d P) {
		double m = 0; 
		if ((P.x - mipos.x) == 0)
			m = (Math.signum(P.y - mipos.y)*Double.MAX_VALUE);
		else 
			m = (P.y - mipos.y)/(P.x - mipos.x);
		return m;
	}*/
	
	public double hallarPendienteRecta(Point3d centro, Point3d p) {
		//if (p.x - centro.x == 0)
			//System.out.println("ahhh pichon");
		double m = (p.y - centro.y)/(p.x - centro.x);
		return m;
	}
	
	
	public double evaluarRecta(double m, Point3d posRobot) {			
		return (m*posRobot.x - posRobot.y);
	}

	public double recta(Point3d posObj, double m, Point3d mipos) {
		double eval = 0;
		if (m == Double.NEGATIVE_INFINITY || m == Double.POSITIVE_INFINITY)
			eval = mipos.x - posObj.x;
		else 
			eval = ((mipos.y - posObj.y) - m*(mipos.x - posObj.x));
		//System.out.println("Evalue la recta en " + eval + " m es " + m);
		return eval;
	}
	
	public double evaluarCircunferencia(Point3d posObjeto, Point3d posRobot) {
		double valor = Math.pow((posRobot.x - posObjeto.x), 2) + Math.pow((posRobot.y - posObjeto.y), 2); 
		return Math.sqrt(valor); 
	}
	
	
	public double getMagnitudFuerza(Point3d mipos, Point3d posObj) {
		double magnitud = 0;
		double distancia = utiles.distanciaEuclidea(mipos, posObj);
		distancia = distancia - tamanioRobot;
		 
		direccionP1 = 0;
		direccionP2 = 0;
		
		if (posObj.z - (3.0/8.0)*Math.PI < -Math.PI)
			direccionP2 = Math.PI + (posObj.z - (3.0/8.0)*Math.PI + Math.PI);
		else
			direccionP2 = posObj.z - (3.0/8.0)*Math.PI;
		
		if (posObj.z + (3.0/8.0)*Math.PI > Math.PI)
			direccionP1 = -Math.PI - (posObj.z - (3.0/8.0)*Math.PI - Math.PI);
		else
			direccionP1 = posObj.z + (3.0/8.0)*Math.PI;
		
		
		Point3d P1 = new Point3d();
		P1.x = Math.cos(direccionP1)*radioCampo + posObj.x;
		P1.y = Math.sin(direccionP1)*radioCampo + posObj.y;
		Point3d P2 = new Point3d();
		P2.x = Math.cos(direccionP2)*radioCampo + posObj.x;
		P2.y = Math.sin(direccionP2)*radioCampo + posObj.y;
		
		m1 = hallarPendienteRecta(posObj, P1);
		m2 = hallarPendienteRecta(posObj, P2);
		
		if (evaluarCircunferencia(posObj, mipos) <= radioCampo) {
			if (m2<0) {
				if (Math.signum(evaluarRecta(m1, mipos) - (m1*posObj.x -posObj.y )) != 
					Math.signum(evaluarRecta(m2, mipos) - (m2*posObj.x -posObj.y ))) {
					magnitud = Krepulsion*((1/distancia) - (1/radioCampo));
					//System.out.println("HAY REPULSION TANGENCIAL 1");
				}
			} else {
				if (Math.signum(evaluarRecta(m1, mipos) - (m1*posObj.x -posObj.y )) == 
					Math.signum(evaluarRecta(m2, mipos) - (m2*posObj.x -posObj.y ))) {
					magnitud = Krepulsion*((1/distancia) - (1/radioCampo));
					//System.out.println("HAY REPULSION TANGENCIAL 2");
				}
			}
		}
		
		
		return magnitud;
	}
	
	
	public double calcularDireccion2(Point3d mipos) throws IOException {
		double direccion = 0;
		
		Point3d posObj = predictor.predecirPosicion(idObjeto);
		direccionP3 = posObj.z;
		Point3d P3 = new Point3d();
		P3.x = Math.cos(direccionP3)*radioCampo + posObj.x;
		P3.y = Math.sin(direccionP3)*radioCampo + posObj.y;
		m3 = hallarPendienteRecta(posObj, P3);
		
		//System.out.println("La direccion de mi robot mirado desde el objetivo es: " + utiles.getAnguloTheta(posObj, mipos) + " y M3 = " + m3);
		
		double anguloRobot = utiles.getAnguloTheta(mipos, posObj);
		
		if (m3 >= 0) {
			if (m2 < 0) {
				//if (Math.signum(evaluarRecta(m2, mipos) - (m2*posObj.x -posObj.y )) != 
					//Math.signum(evaluarRecta(m3, mipos) - (m3*posObj.x -posObj.y )) || (P3.x == posObj.x))
				if (Math.signum(recta(posObj, m2, mipos)) != Math.signum(recta(posObj, m3, mipos))) 					
				{	
					direccion = anguloRobot + Math.PI/2;
					if (direccion > Math.PI)
						direccion = -Math.PI + (direccion - Math.PI);
					//System.out.println(" + 90 con m3 >= 0 y m2 < 0");
				} else {
					direccion = anguloRobot - Math.PI/2;
					if (direccion < -Math.PI)
						direccion = Math.PI + (direccion + Math.PI);
					//System.out.println(" - 90 con m3 >= 0 y m2 < 0");
				}
			} else {
				//if (Math.signum(evaluarRecta(m2, mipos) - (m2*posObj.x -posObj.y )) == 
					//Math.signum(evaluarRecta(m3, mipos) - (m3*posObj.x -posObj.y )) ||
					//(P3.x == posObj.x))
				if (Math.signum(recta(posObj, m2, mipos)) != Math.signum(recta(posObj, m3, mipos)))
				{	
					direccion = anguloRobot + Math.PI/2;
					if (direccion > Math.PI)
						direccion = -Math.PI + (direccion - Math.PI);
					//System.out.println(" + 90 con m3 >= 0 y m2 >= 0");
				} else {
					direccion = anguloRobot - Math.PI/2;
					if (direccion < -Math.PI)
						direccion = Math.PI + (direccion + Math.PI);
					//System.out.println(" - 90 con m3 >= 0 y m2 >= 0");
				}
			}
										
		} else {
			if (m2 >= 0) {
				//if (Math.signum(evaluarRecta(m2, mipos) - (m2*posObj.x -posObj.y )) == 
					//Math.signum(evaluarRecta(m3, mipos) - (m3*posObj.x -posObj.y )) || 
					//((P3.y == posObj.y)))
				if (Math.signum(recta(posObj, m2, mipos)) == Math.signum(recta(posObj, m3, mipos)))
				{	
					direccion = anguloRobot + Math.PI/2;
					if (direccion > Math.PI)
						direccion = -Math.PI + (direccion - Math.PI);
					//System.out.println(" + 90 con m3 < 0 y m2 >= 0");
				} else {
					direccion = anguloRobot - Math.PI/2;
					if (direccion < -Math.PI)
						direccion = Math.PI + (direccion + Math.PI);
					//System.out.println(" - 90 con m3 < 0 y m2 >= 0 y el semiplano de m3 es " + recta(posObj, m3, mipos) + " y el de m2 es: " + recta(posObj, m2, mipos));
				}
			} else {
				//if (Math.signum(evaluarRecta(m2, mipos) - (m2*posObj.x -posObj.y )) != 
					//Math.signum(evaluarRecta(m3, mipos) - (m3*posObj.x -posObj.y )) || 
					//((P3.y == posObj.y)))
				if (Math.signum(recta(posObj, m2, mipos)) != Math.signum(recta(posObj, m3, mipos)))
				{	
					direccion = anguloRobot + Math.PI/2;
					if (direccion > Math.PI)
						direccion = -Math.PI + (direccion - Math.PI);
					//System.out.println(" + 90 con m3 < 0 y m2 >= 0");
				} else {
					direccion = anguloRobot - Math.PI/2;
					if (direccion < -Math.PI)
						direccion = Math.PI + (direccion + Math.PI);
					//System.out.println(" - 90 con m3 < 0 y m2 < 0 y el semiplano de m3 es " + recta(posObj, m3, mipos) + " y el de m2 es: " + recta(posObj, m2, mipos));
				}
			}
		}
					
			
		
		return direccion;
	}
	
	public double calcularDireccion(Point3d mipos) throws IOException {
		double direccion = 0;
		
		Point3d posObj = predictor.predecirPosicion(idObjeto);
		direccionP3 = posObj.z;
		Point3d P3 = new Point3d();
		P3.x = Math.cos(direccionP3)*radioCampo + posObj.x;
		P3.y = Math.sin(direccionP3)*radioCampo + posObj.y;
		m3 = hallarPendienteRecta(posObj, P3);
		
		//System.out.println("La direccion de mi robot mirado desde el objetivo es: " + utiles.getAnguloTheta(posObj, mipos) + " y M3 = " + m3);
		
		double anguloRobot = utiles.getAnguloTheta(mipos, posObj);
		
		if (m3 >= 0) {
			if (m2 < 0) {
				if (Math.signum(evaluarRecta(m2, mipos) - (m2*posObj.x -posObj.y )) != 
					Math.signum(evaluarRecta(m3, mipos) - (m3*posObj.x -posObj.y )) || (P3.x == posObj.x))			
				{	
					direccion = anguloRobot + Math.PI/2;
					if (direccion > Math.PI)
						direccion = -Math.PI + (direccion - Math.PI);
					//System.out.println(" + 90 con m3 >= 0");
				} else {
					direccion = anguloRobot - Math.PI/2;
					if (direccion < -Math.PI)
						direccion = Math.PI + (direccion + Math.PI);
					//System.out.println(" - 90 con m3 >= 0");
				}
			} else {
				if (Math.signum(evaluarRecta(m2, mipos) - (m2*posObj.x -posObj.y )) == 
					Math.signum(evaluarRecta(m3, mipos) - (m3*posObj.x -posObj.y )) ||
					(P3.x == posObj.x))
				{	
					direccion = anguloRobot + Math.PI/2;
					if (direccion > Math.PI)
						direccion = -Math.PI + (direccion - Math.PI);
					//System.out.println(" + 90 con m3 >= 0");
				} else {
					direccion = anguloRobot - Math.PI/2;
					if (direccion < -Math.PI)
						direccion = Math.PI + (direccion + Math.PI);
					//System.out.println(" - 90 con m3 >= 0");
				}
			}
										
		} else {
			if (m2 >= 0) {
				if (Math.signum(evaluarRecta(m2, mipos) - (m2*posObj.x -posObj.y )) == 
					Math.signum(evaluarRecta(m3, mipos) - (m3*posObj.x -posObj.y )) || 
					((P3.y == posObj.y)))
				{	
					direccion = anguloRobot + Math.PI/2;
					if (direccion > Math.PI)
						direccion = -Math.PI + (direccion - Math.PI);
					//System.out.println(" + 90 con m3 < 0");
				} else {
					direccion = anguloRobot - Math.PI/2;
					if (direccion < -Math.PI)
						direccion = Math.PI + (direccion + Math.PI);
					//System.out.println(" - 90 con m3 < 0 y el Semiplano de m3 es " + Math.signum((m3*posObj.x -posObj.y) - evaluarRecta(m3, mipos)) + "y el de m2 es: " + Math.signum((m2*posObj.x -posObj.y) - evaluarRecta(m2, mipos)));
				}
			} else {
				if (Math.signum(evaluarRecta(m2, mipos) - (m2*posObj.x -posObj.y )) != 
					Math.signum(evaluarRecta(m3, mipos) - (m3*posObj.x -posObj.y )) || 
					((P3.y == posObj.y)))
				{	
					direccion = anguloRobot + Math.PI/2;
					if (direccion > Math.PI)
						direccion = -Math.PI + (direccion - Math.PI);
					//System.out.println(" + 90 con m3 < 0");
				} else {
					direccion = anguloRobot - Math.PI/2;
					if (direccion < -Math.PI)
						direccion = Math.PI + (direccion + Math.PI);
					//System.out.println(" - 90 con m3 < 0 y el Semiplano de m3 es " + Math.signum((m3*posObj.x -posObj.y) - evaluarRecta(m3, mipos)) + "y el de m2 es: " + Math.signum((m2*posObj.x -posObj.y) - evaluarRecta(m2, mipos)));
				}
			}
		}
					
			
		
		return direccion;
	}
	
	public Vector obtenerFuerza(Point3d mipos) throws IOException{
		cargarConstantes();
		
		double magnitud = getMagnitudFuerza(mipos, predictor.predecirPosicion(idObjeto));
		
		double direccion = calcularDireccion2(mipos);
		
		//System.out.println("REPULSOR tangencial " + idObjeto + ": direccion = " + direccion + " magnitud = " + magnitud);
		 
		Vector resultante = new Vector(magnitud, direccion);  
		
		resultante.setMagnitud(Math.abs(magnitud));
		
		return resultante;
	};
}

